/*
 * CLProtocol.cpp
 *
 *  Created on: May 20, 2011
 *      Author: ulas
 */

#include "CLProtocol.h"
#include <stdio.h>
#include <unistd.h>
#include <pthread.h>

#define DEVICE_FILE "/dev/hello"

BOOL quit = FALSE;    // signal flag

void got_signal(int)
{
    quit = TRUE;
}

CLProtocol::CLProtocol(U8 nodeId)
{
	DBG("CL Protocol Initializing\n");
	notRTSlot[0] = NON_REAL_TIME_SLOT;
	notRTSlot[1] = 0x00;
	notMyRTSlot[0] = REAL_TIME_SLOT;
	notMyRTSlot[1] = 0X00;
	opts = new rtOpts();
	NOTIFY("nodeId: %d", nodeId);
	NODE_ID = nodeId;
	dbgCnt = 0;

	int i;
	for(i = 0 ; i < LATENCY_ARRAY_COUNT; i++) {
		curLatency[i] = 0;
		maxLatency[i] = 0;
		minLatency[i] = 100000;
		avgLatency[i] = 0;
		cntLatency[i] = 0;
		updatedLatency[i] = false;
	}
}

STATUS CLProtocol::run()
{
	U32 ret;
	int retRead;
	int length;
	TimeInternal timeout = { 5, 0};
	TimeInternal sendTimeStamp, tdiff,receiveTime;
	char msg[MQUEUE_MAX_MSG_SIZE];
	memset(msg, 0x00, MQUEUE_MAX_MSG_SIZE);
	initTimer();
	IntervalTimer  itimer[INTERVAL_TIMER_SIZE];
	timerStart(REQRT_INTERVAL_TIMER, 1,  itimer);


	int dev_file = open(DEVICE_FILE, O_RDONLY ); //| O_NONBLOCK);
	if (dev_file < 0)
	{
		PERROR("[FAIL] error opening device file %s\n", strerror(errno));
		exit(-1);
	}
	NOTIFY("[OK] device file %s opened\n", DEVICE_FILE);

	struct sigaction sa;
	memset( &sa, 0, sizeof(sa) );
	sa.sa_handler = got_signal;
	sigfillset(&sa.sa_mask);
	sigaction(SIGINT,&sa,NULL);
	sigaction(SIGTERM,&sa,NULL);
	sigaction(SIGTSTP,&sa, NULL);

	for(;;) {
		ret = selectMsgQ(&timeout, dev_file);
		if(ret < 0) {
			PERROR("failed to poll sockets");
		}
		else if(ret > 0){
			length = mq_receive(opts->mqList[MQUEUE_IL2CLREQ_ind], msg, MQUEUE_MAX_MSG_SIZE, 0);
			if(length > 0)
			{
				getTime(&receiveTime);
				sendTimeStamp.seconds 		= *(U32*)&msg[1];
				sendTimeStamp.nanoseconds 	= *(U32*)&msg[5];
				tdiff  = subtractTime(&receiveTime, &sendTimeStamp);

				processREQRT();
				updateLatencyData(IL2CLREQRT, &tdiff );
			}
			else{
				length = mq_receive(opts->mqList[MQUEUE_IL2CLRT_ind], msg, MQUEUE_MAX_MSG_SIZE, 0);
				if(length > 0)
				{
					DBG("IL2CLRT received: <%d>, %d\n", msg[length-1], length);
					processIL2CLRT((U8*)msg, length);
				}
				else {
					length = mq_receive(opts->mqList[MQUEUE_AP2CL_ind], msg, MQUEUE_MAX_MSG_SIZE, 0);
					if(length > 0)
					{
						DBG("AP2CL received: <%c>, %d\n", msg[length-1], length);
						processAP2CL((U8*)msg, length);
					}
					else {
						retRead = read(dev_file, &msg, 15);
						getTime(&receiveTime);

						sendTimeStamp.seconds 		= *(U32*)&msg[1];
						sendTimeStamp.nanoseconds 	= *(U32*)&msg[5];
						tdiff  = subtractTime(&receiveTime, &sendTimeStamp);

						if(retRead == -1)
							strerror(errno);
						else
						{
							updateLatencyData(IL_MODULE, &tdiff );
						}
					}
				}
			}
		}
		else {
			DBG("handle nothing\n");
		}


		if( quit ) {
			close(dev_file);
			break;
		}
	}
	return OK;
}

void CLProtocol::updateLatencyData(U8 latencyArrSelect, TimeInternal* tdiff)
{
	updatedLatency[latencyArrSelect] = true;
	curLatency[latencyArrSelect] = tdiff->nanoseconds;
	if(curLatency[latencyArrSelect] > maxLatency[latencyArrSelect]) {
		maxLatency[latencyArrSelect] = curLatency[latencyArrSelect];
	}
	if(curLatency[latencyArrSelect] < minLatency[latencyArrSelect]) {
		minLatency[latencyArrSelect] = curLatency[latencyArrSelect];
	}
	avgLatency[latencyArrSelect] = (avgLatency[latencyArrSelect]*cntLatency[latencyArrSelect] + curLatency[latencyArrSelect])/(cntLatency[latencyArrSelect]+1);
	cntLatency[latencyArrSelect]++;
}

U8 CLProtocol::selectMsgQ(TimeInternal* timeout, int dev_file)
{
	U32 i;
	int ret, nfds = 0;
	fd_set readfds;
	struct timeval tv, *tv_ptr;

	if (timeout < 0)
		return FALSE;

	FD_ZERO(&readfds);
	for(i=2; i<MQUEUE_COUNT; i++) {
		if(opts->mqList[i] > nfds)
			nfds = opts->mqList[i];
		FD_SET(opts->mqList[i], &readfds);
	}
	FD_SET(dev_file, &readfds);
	if(dev_file > nfds) {
		nfds = dev_file;
	}

	if (timeout) {
		tv.tv_sec = timeout->seconds;
		tv.tv_usec = timeout->nanoseconds / 1000;
		tv_ptr = &tv;
	} else
		tv_ptr = 0;

	ret = select(nfds + 1, &readfds, 0, 0, tv_ptr) > 0;

	if (ret < 0) {
		if (errno == EAGAIN || errno == EINTR)
			return 0;
	}
	return ret;
}

STATUS CLProtocol::processAP2CL(U8* msg, U16 msgLength)
{
	U8 ch = msg[0];
	if( msg == NULL || msgLength == 0 || ch == 0 || ch > TOTAL_NUMBER_OF_CHANNELS )
	{
		PERROR("processAP2CL || RECEIVED INVALID MESSAGE CANNOT ADD TO TX]" );
		return ERROR;
	}
	if(opts->Tx[ch].msgLength != 0) {
		INFO("OVERRIDING THE NON-EMPTY TX[%d]", ch );
	}

	memcpy(&(opts->Tx[ch].msg[0]), &msg[1], msgLength - 1);
	opts->Tx[ch].msgLength = msgLength - 1;

	return OK;
}


STATUS CLProtocol::processIL2CLRT(const U8* msg, const U16 msgLength)
{
	U8* receivedMsg;
	/*this may be the case where IL did not received a RT message even though the slot is RT*/
	if( msg == NULL || msgLength == 0)
	{
		DBG("processIL2CLRT received a null message\n");
		gUpdate(msg, 0);
		return ERROR;
	}

	/*enqueue received message into the queue*/
	if( (receivedMsg = (U8*)malloc( msgLength*sizeof(U8)) ) == NULL)
	{
		PERROR("!!ERROR Cannot Allocate Memory for Received Message!!");
		exit(EXIT_FAILURE);
	}
	memcpy( receivedMsg, msg, msgLength);
	opts->insertIntoRxQueue(receivedMsg);

	gUpdate(msg, msgLength);

    return OK;

}

STATUS CLProtocol::processREQRT()
{
	TimeInternal t1,t2, tdif;
	getTime(&t1);
	int mqSendResult;

	gRealTime(&(opts->RTCL));
	if(opts->RTCL == REAL_TIME_SLOT)
		gMySlot(&(opts->myCL));

	/*update cnt variable used by DART after performing REQRT*/
	updateCountForDART();


	opts->reqCL = TRUE;

	getTime(&t2);
	tdif = subtractTime(&t2, &t1);

	dbgCnt++;
	notRTSlot[0] = dbgCnt;
	notMyRTSlot[0] = dbgCnt;


	/*redundant parameter*/
	if(opts->reqCL == FALSE )
	{
		PERROR("CL2ILTask || reqCL is NOT TRUE");
		return ERROR;
	}

	DBG("time difference: %d sec %d nsec\n", tdif.seconds, tdif.nanoseconds);
	if ( tdif.nanoseconds < DEL && tdif.seconds == 0  ) //&& t2-t1 <= del
	{
		if( opts->RTCL == NON_REAL_TIME_SLOT )
		{
			DBG("CL2ILTask || Not a RT Slot \n");
			mq_send(opts->mqList[MQUEUE_CL2IL_ind], (char*)notRTSlot, 2, 0);//
		}
		else if( opts->myCL.mySlot == TRUE)
		{
            DBG("CL2ILTask || RT slot is BELONGED to this node \n");

			if( (opts->Tx[opts->myCL.ch]).msgLength != 0 )
			{
				mq_send(opts->mqList[MQUEUE_CL2IL_ind], (char*)&(opts->Tx[opts->myCL.ch].msg[0]), opts->Tx[opts->myCL.ch].msgLength, 0);
				opts->Tx[opts->myCL.ch].msgLength = 0;
            }
            else
            {
                 DBG("CL2ILTask: no msg to transmit \n");
     			 mq_send(opts->mqList[MQUEUE_CL2IL_ind], (char*)notMyRTSlot, 2, 0);//
            }
		}
		else {
			mqSendResult = mq_send(opts->mqList[MQUEUE_CL2IL_ind], (char*)notMyRTSlot, 2, 0);//
            DBG("CL2ILTask || RT slot NOT BELONGED to this node %d\n",mqSendResult);
		}
		opts->reqCL = FALSE;
	}
	else
		NOTIFY("PROCESSING TIME IS LARGER THAN del: %9d.%9d\n", tdif.seconds, tdif.nanoseconds);
	return OK;

}


CLProtocol::~CLProtocol()
{
	delete opts;
}
